/**
 ******************************************************************************
 * @file    mini_dog.h
 * @author  GEEKROS,  site:www.geekros.com
 ******************************************************************************
 */

#ifndef ROBOT_MINI_DOG
#define ROBOT_MINI_DOG

#include "utils.h"
#include "adc.h"
#include "servo.h"
#include "mpu.h"
#include "wifi.h"

#include "mini_dog_app.h"
#include "mini_dog_move.h"
#include "mini_dog_order.h"
#include "mini_dog_remote.h"

#define Dog_Delay_Time_Set 5

#define Dog_C_Leg_Anlge_Count 1*20/13
#define Dog_C_Leg_Servo_Count 1*13/20

#define Dog_Init_Height 80.00f
#define Dog_Init_Gravity_X 0.00f
#define Dog_Init_Gravity_Y 0.00f
#define Dog_Init_Error 5.00f

#define Dog_Pit_Max 20.2f
#define Dog_Pit_Min 6.6f
#define Dog_Roll_Max 20.6f
#define Dog_Roll_Min 3.6f
#define Dog_Yaw_Max 6.6f
#define Dog_Yaw_Min 3.6f


#define Dog_Pit_Error 0.0f
#define Dog_Roll_Error 0.0f
#define Dog_Yaw_Error 0.0f

#define PI 3.1415926f

#define Lite_Dog_Len 12
#define Lite_Dog_Pi 3.14159f
#define Lite_Dog_Angle_To_Digital 3.41f
#define Lite_Dog_Digital_TO_Angle 0.293f
#define	Lite_Dog_Gait_Cycle	1 //步态周期
#define Lite_Dog_Gait_Duty_Cycle 0.5f 	//支撑向占空比

#define Max_Time_Set 10000

#define Dog_Move_Max_Height 14.00f
#define Dog_Move_Max_Lenth 40.00f
#define Dog_Move_Max_Control_Lenth 30.00f
#define Dog_Move_Max_Speed 40.00f
#define Dog_Move_Control_Error 10.00f

//备注是向下动是增加还是减少
#define Dog_LF_A_Servo_Middle_Data 460 //+
#define Dog_RF_A_Servo_Middle_Data 415 //-
#define Dog_LB_A_Servo_Middle_Data 484 //-
#define Dog_RB_A_Servo_Middle_Data 455 //+
 
#define Dog_LF_B_Servo_Middle_Data 314 //+
#define Dog_RF_B_Servo_Middle_Data 581 //-
#define Dog_LB_B_Servo_Middle_Data 461 //+
#define Dog_RB_B_Servo_Middle_Data 381 //-

#define Dog_LF_C_Servo_Middle_Data 420 //+
#define Dog_RF_C_Servo_Middle_Data 490 //-
#define Dog_LB_C_Servo_Middle_Data 430 //+
#define Dog_RB_C_Servo_Middle_Data 350 //-

//180 min
#define Dog_LF_B_Servo_Min_Data 134 //+
#define Dog_RF_B_Servo_Min_Data 751 //-
#define Dog_LB_B_Servo_Min_Data 221 //+
#define Dog_RB_B_Servo_Min_Data 566 //-

//170 min
#define Dog_LF_C_Servo_Min_Data 254 //+
#define Dog_RF_C_Servo_Min_Data 677 //-
#define Dog_LB_C_Servo_Min_Data 256 //+
#define Dog_RB_C_Servo_Min_Data 428 //-

//差值180
#define Dog_LF_B_Servo_Max_Data 449 //+
#define Dog_RF_B_Servo_Max_Data 442 //-
#define Dog_LB_B_Servo_Max_Data 545 //+
#define Dog_RB_B_Servo_Max_Data 220 //-

//170
#define Dog_LF_C_Servo_Max_Data 558 //+
#define Dog_RF_C_Servo_Max_Data 358 //-
#define Dog_LB_C_Servo_Max_Data 577 //+
#define Dog_RB_C_Servo_Max_Data 118 //-

#define Dog_LF_BC_Error_Min_Data -116 //+
#define Dog_RF_BC_Error_Min_Data 74 //-
#define Dog_LB_BC_Error_Min_Data 24 //+
#define Dog_RB_BC_Error_Min_Data 134 //- 

#define Dog_LF_BC_Error_Max_Data -104 //+
#define Dog_RF_BC_Error_Max_Data 84 //-
#define Dog_LB_BC_Error_Max_Data 24 //+
#define Dog_RB_BC_Error_Max_Data 103 //- 

//170
#define Dog_LF_BC_Error_Data(Now_B_Data) (((Now_B_Data)-Dog_LF_B_Servo_Min_Data)/(Dog_LF_B_Servo_Max_Data-Dog_LF_B_Servo_Min_Data)*\
										(Dog_LF_BC_Error_Max_Data - Dog_LF_BC_Error_Min_Data) + Dog_LF_BC_Error_Min_Data)//+
#define Dog_RF_BC_Error_Data(Now_B_Data) (((Now_B_Data)-Dog_RF_B_Servo_Min_Data)/(Dog_RF_B_Servo_Max_Data-Dog_RF_B_Servo_Min_Data)*\
										(Dog_RF_BC_Error_Max_Data - Dog_RF_BC_Error_Min_Data) + Dog_RF_BC_Error_Min_Data)
#define Dog_LB_BC_Error_Data(Now_B_Data) (((Now_B_Data)-Dog_LB_B_Servo_Min_Data)/(Dog_LB_B_Servo_Max_Data-Dog_LB_B_Servo_Min_Data)*\
										(Dog_LB_BC_Error_Max_Data - Dog_LB_BC_Error_Min_Data) + Dog_LB_BC_Error_Min_Data)
#define Dog_RB_BC_Error_Data(Now_B_Data) (((Now_B_Data)-Dog_RB_B_Servo_Min_Data)/(Dog_RB_B_Servo_Max_Data-Dog_RB_B_Servo_Min_Data)*\
										(Dog_RB_BC_Error_Max_Data - Dog_RB_BC_Error_Min_Data) + Dog_RB_BC_Error_Min_Data)


#define Dog_C_Angle_Min 25.9163

//舵狗当前状态
/******
 * 正常状态
 * 准备状态
 * 暂停状态
 * 错误状态
 * 完成状态
 ******/
typedef enum
{
	Dog_OK = 0,
	Dog_Ready = 1,
	Dog_Stop = 2,
	Dog_Error = 3,
	Dog_Done = 4,
	Dog_Celebrate = 5,
	Dog_PowerLess = 6,
	Dog_OffsetData_Loss = 7,
	Dog_Auto = 8
}Dog_State_Enum;

typedef enum
{
	Leg_OK = 0,
	Leg_Ready = 1,//软
	Leg_Butt_Celebrate = 2,// 校准大腿根部舵机
	Leg_Thigh_Celebrate = 3,// 将腿摆到最下面
	Leg_Calf_Celebrate = 4,// 将腿摆到最上面
	Leg_Celebrate_Done = 5,
	Leg_Stop = 6,//加力，固定
	Leg_Error = 7,
}Leg_Celebrate_State_Enum;

typedef enum
{
	Dog_LF_Leg = 0,
	Dog_RF_Leg = 1,
	Dog_LB_Leg = 2,
	Dog_RB_Leg = 3
}Leg_Loc_Enum;//腿部位置枚举

typedef enum
{
    Lite_Dog_Trot = 1,
    Lite_Dog_Walk,
	Lite_Dog_Pronk,
}Lite_Dog_Gait_Mode; //模态模式枚举

typedef enum
{
	Dog_Init_Mode = 0,
	Dog_Calibration_Mode = 1,
	Dog_Auto_Mode,
	Dog_Remote_Control_Mode,
	Dog_Order_Control_Mode,
	Dog_Recovery_Mode,
	Dog_PowerLess_Mode
}Lite_Dog_Mode_Enum; //控制模式枚举

//舵狗遥控模式
/******
 * 自适应角度模式
 * 姿态控制模式
 * 动态运动模式
 * 遥控校准模式
 * 舵机无力模式
 ******/
typedef enum
{
	Dog_RM_PowerLess_Mode = 0,
	Dog_RM_Att_Control_Mode = 1,
	Dog_RM_Dyna_Motion_Mode = 2,
	Dog_RM_Celebrate_Mode = 3,
	Dog_RM_Adaptive_Angle_Mode = 4,
	Dog_RM_Omni_Motion_Mode = 5
}Dog_Remote_Mode_t;

typedef struct {
	float roll;
	float pitch;
	float yaw;
	float x;
	float y;
	float z;
} Lite_Dog_Position_Attitude_Struct; //位置和姿态结构体

typedef struct {
	float x[4];
	float y[4];
	float z[4];
} Lite_Dog_Foot_Coordinate_Struct; //足端坐标结构体

typedef struct {
	float butt[4];
	float thigh[4];
	float calf[4];
} Lite_Dog_Leg_Angle_Struct; //大小腿关节角结构体

typedef struct
{
	int servo_len; //舵机数量
	float servo_angle_range; //舵机角度范围
	float body_length; //身体长度
	float body_width; //身体宽度
	float foot_width; //足宽度
	float butt_length; //大腿长度
	float thigh_length; //大腿长度
	float calf_length; //小腿长度
	uint16_t zero_position[Lite_Dog_Len]; //校准后的零位数据
	int joint_offset; //关节偏移量

	float init_height;

	float pitch_error;
	float roll_error;
	float yaw_error;

	float pitch_max;
	float roll_max;
	float yaw_max;

	float pitch_add;
	float roll_add;
	float yaw_add;	

	float x_max;
	float y_max;
	float z_max;

	float x_min;
	float y_min;
	float z_min;

}Lite_Dog_Config_Struct;

//单腿数据
typedef struct
{	
	Leg_Loc_Enum Leg_Loc;
	Servo_Struct* Leg_Motor_Message[3];
	float Leg_Angle_Now[3];
	float Leg_Angle_Set[3];//设置的角度,PI
	int Leg_Servo_Pos_Set[3];
	int Leg_Offset_Data[3];
	float Set_Leg_Loc[3];//	
	float Now_Leg_Loc[3];//
}Dog_Leg_Data_Struct;

//舵狗运动状态
typedef enum
{
	Trot_M = 0,
	Walk_M = 1,
	Pronk_M = 2,
	Stable_M = 3,
	Cailbration_M = 4
}Dog_Sport_Mode_Enum;

//舵狗运动模式
typedef enum
{
	Stop_M = 0,
	Front_M = 1,
	Turn_Left_M = 2,
	Turn_Right_M = 3,
	Walk_Left_M = 4,
	Walk_Right_M = 5,
	Back_M = 6,
	Step_M = 7,
	Omni_M = 8,
	Auto_M = 9
}Dog_Move_Mode_Enum;

//狗当前姿态，是否倾倒
typedef enum
{
	Body_All_Up = 0,
	Body_Left_Down = 1,
	Body_Right_Down = 2,
	Body_All_Down = 3
}Dog_Body_Now_Attitude_Enum;

//狗的姿态数据
typedef struct
{
	const Mpu_Read_Struct* Dog_Mpu_Data;
	const float* Dog_Body_Set_Cood[4];
	const float* Dog_Body_Now_Cood[4];
	
	float Dog_Now_Height;
	float Dog_Now_Gravity;
	float Dog_Now_Roll;//
	float Dog_Now_Pitch;
	float Dog_Now_Yaw;

	float Dog_Now_Roll_WOMpu;
	float Dog_Now_Pitch_WOMpu;
	float Dog_Now_Yaw_WOMpu;

	Dog_Body_Now_Attitude_Enum Dog_Body_Now_Att;
}Dog_State_Data_Struct;

//狗的动态控制数据
typedef struct
{
	Lite_Dog_Config_Struct* Dog_Config_Data;
	Dog_Sport_Mode_Enum Dog_Sport_Mode;	
	Dog_Move_Mode_Enum Dog_Move_Mode;
	int Leg_L_Move_State;
	int Leg_R_Move_State;
	float Move_Speed_Set;
	float Move_Height_Set;
	float Move_Lenth_Set;

	float Leg_Now_Move_Height;
	float Leg_Now_Move_Lenth;
	float Leg_L_Now_Move_State;
	float Leg_R_Now_Move_State;

	int	Single_Leg_Move_Time;
	int All_Leg_Move_Time;
	float Leg_Move_Duty;
	int Leg_Move_Flag;

	int Motion_Count_Time;
	int Move_Start_Flag;

	float Set_Omni_Angle;
	float Now_Omni_Angle;

	float Leg_Stop_XY_Cood[3][4];
}Dog_Set_Move_Data_Struct;

//舵狗预设指令动作
/******
 * 俯卧撑模式
 * 握个手打招呼模式
 * 扭一扭模式
 * 随地大小便
 * 自定义动作
 ******/
typedef enum
{
	PushUp_Auto = 0,
	SayHello_Auto = 1,
	Dance_Auto = 2,
	Pee_Auto = 3,
	New_Auto_Motion1 = 4,
	New_Auto_Motion2 = 5,
	New_Auto_Motion3 = 6
}Dog_Order_Auto_Mode_Struct;

//舵狗指令模式
/******
 * 指令运动模式
 * 指令自稳模式
 * 指令校准模式
 * 指令自助翻转
 * 指令自动模式
 ******/
typedef enum
{
	Dog_Order_Move_Mode = 0,
	Dog_Order_Stable_Mode = 1,
	Dog_Order_Celebrate_Mode = 2,
	Dog_Order_Auto_Mode = 3
}Dog_Order_Set_Mode_Struct;



//狗的指令控制数据
typedef struct
{
	Dog_Order_Set_Mode_Struct Dog_Order_Set_Mode;
	Dog_Order_Auto_Mode_Struct Dog_Order_Auto;
	Dog_Sport_Mode_Enum Dog_Order_Sport_Mode;	
	Dog_Move_Mode_Enum Dog_Order_Move_Mode;

	Dog_State_Enum Dog_Order_Auto_State;

	float Order_Mode_Move_Speed_Set;
	float Order_Mode_Move_Height_Set;
	float Order_Mode_Move_Lenth_Set;

	float Order_Mode_XYZ_Set[3];

	float Order_Mode_Roll_Set;
	float Order_Mode_Pitch_Set;
	float Order_Mode_Yaw_Set;

	Leg_Loc_Enum Leg_Celebrate_Loc_Set;
	Leg_Celebrate_State_Enum Leg_Celebrate_State;
	
	float Order_Omni_Angle_Set;

	float Auto_Down_Flag;
}Dog_Order_Mode_Struct;

//狗的控制数据
typedef struct
{
	Dog_Remote_Mode_t Dog_Remote_Mode;//遥控器模式枚举	
	Dog_Order_Mode_Struct* Dog_Order_Mode_Data;//指令模式数据
	const Rocker_Struct* Dog_Remote_Control;
	Dog_Set_Move_Data_Struct Dog_Set_Move_Data;
	float Dog_Body_Cood_Set[4][2];	
}Dog_Control_Data_Struct;

typedef struct
{
	// 机器狗四条腿相关数据结构体
	Dog_Leg_Data_Struct Dog_LF_Leg_Data;
	Dog_Leg_Data_Struct Dog_RF_Leg_Data;
	Dog_Leg_Data_Struct Dog_LB_Leg_Data;
	Dog_Leg_Data_Struct Dog_RB_Leg_Data;
	// 机器狗整体状态相关数据结构体
	Dog_State_Data_Struct Dog_State_Data;

	// 设置机器狗四足足端坐标数据地址
	float Dog_Set_XY_Coordinate[3][4];//X,Y,Z

	// 机器狗当前状态
	Dog_State_Enum Dog_State;//状态

	// flash相关数据存储
	float Lite_Dog_Flash_Data[12];

	// 机器狗相关数据配置信息
	Lite_Dog_Config_Struct Dog_Config_Data;
	
	// 机器狗控制模式枚举体
	Lite_Dog_Mode_Enum Dog_Mode;

	// 机器狗控制相关数结构体
	Dog_Control_Data_Struct Dog_Control_Data;
	
	// 机器狗目标姿态相关数据结构体
	Lite_Dog_Position_Attitude_Struct Target_Attitude; 

	// 机器狗四条腿关节角
	Lite_Dog_Leg_Angle_Struct leg_angle; //大小腿关节角

}Lite_Dog_Struct;

Lite_Dog_Struct* Get_Lite_Dog_Data_Address(void);

void Mini_Dog_Init(void);

void Mini_Dog_Task(void);

void Lite_Dog_Init_Motion_Set(Lite_Dog_Struct* Dog_Init);

void Lite_Dog_Change_Mode(Lite_Dog_Mode_Enum Dog_Mode_Change);

#endif
